

/*-------------------------------------------------------
                 Headerfiles 
---------------------------------------------------------*/
#include <REG167.H>
#include "Decoder.h"
#include "mem_access.h"
#include "uart.h"
#include "protokoll.h"
long Get24BitVal(char *startAdr)
{
	long retVal = 0;
	int i;
	for (i = 2; i >= 0; i--)
	{
		retVal <<= 8;
		retVal += startAdr[i];
	}
	return retVal;
}
void Decode(char* buf, int len)  // buf is data directly, no status, no length included
{
	int i = 0;
	int errorFlag = 0;
	int bufPos = 0;
	char length = 0;
	char Command;
	char* srcAdr = 0;
	int readValue = 0;
	len = len;
	Command = buf[0] & 0x7F;
	switch(Command) { // Function Select, lower 7 bit
		// COM TEST
		case 1:
			if(buf[1] == (char)0xAA && buf[2] == (char)0x55) {
				// Send Response
				txBuf[1] = 5; // length
				txBuf[2] = 1; // comm test
				txBuf[3] = (char)0x55;
				txBuf[4] = (char)0xAA;
				bufPos = 5;
			}
			else {
			
			}
			break;
		// READ  // WRITE
		case 2:
		case 3:
			bufPos = 1;
			// get srcAddress/dstAddress. Address and length is the same for read and write
			srcAdr = (char*)Get24BitVal(&buf[bufPos]);
			bufPos += 3;
			// get Length
			length = buf[bufPos];

			prepareResponseHeader((long)srcAdr, length);
			if (Command == 3)
			{
				// Call write mem
				if( write_mem(srcAdr,&buf[5],length) < 256) {
					txBuf[1] = 7; // length
					txBuf[2] = 3; // write
					bufPos = 7;
				}
				else {
				// ERROR
				
				}
			}
			else
			{			
				// Reading Loop
				bufPos = 7;
				txBuf[1] = 7+length;
				txBuf[2] = 2;  // read
				for(i = 0; i < length; i++) {
					// Call Read Memory Fnc
					readValue = read_mem(srcAdr + i);  // TODO: check for signed/unsigned!
					if(readValue < 256) {
						// OK
						txBuf[bufPos] = (char)readValue;
						bufPos += 1;
					}
					else {
						errorFlag = 1;
					}
				}
			}				
			// Send Response 1
			if(errorFlag == 1) { // ERROR
				// TODO
			}
			else {	
			}
			break;
		
		// ERROR
		default:
			break;
	}
	// Send Response
	NewProtokollPart(txBuf);

}

void prepareResponseHeader(long startAdr, int length) {
			
	// Prepare Response : StartAddress & Length
	txBuf[3] = (char)  startAdr & 0xFF;
	txBuf[4] = (char)  (startAdr>>8) & 0xFF ;
	txBuf[5] = (char)  (startAdr>>16) & 0xFF ;
	
	txBuf[6] = (char)length;
}
